#include "../../include/interface/CheatIO.h"
#include "../../include/interface/KeyBoard.h"
#include <iostream>
#include <unistd.h>
#include <csignal>

#include "msg_process/receive_data.h"
#include "msg_process/send_data.h"

inline void RosShutDown(int sig){
    ROS_INFO("ROS interface shutting down!");
	ros::shutdown();
}

CheatIO::CheatIO(std::string robot_name):IOInterface(), _subSpinner(1)
{
    // int argc; char **argv;
    // ros::init(argc, argv, "unitree_gazebo_servo");
    std::cout << "The control interface for ROS Gazebo simulation with cheat states from gazebo" << std::endl;
    _robot_name = robot_name;

    // start subscriber
    initRecv();
    _subSpinner.start();
    usleep(3000);     //wait for subscribers start
    // initialize publisher
    initSend();   
    signal(SIGINT, RosShutDown);
    cmdPanel = new KeyBoard();
}

CheatIO::~CheatIO()
{
    ros::shutdown();
}

void CheatIO::sendRecv(const LowlevelCmd *cmd, LowlevelState *state)
{
    sendCmd(cmd);
    recvState(state);
    cmdPanel->updateVelCmd(state);

    state->userCmd = cmdPanel->getUserCmd();
    state->userValue = cmdPanel->getUserValue();
}

void CheatIO::sendCmd(const LowlevelCmd *cmd)
{
    for(int i = 0; i < 10; i++){
        _lowCmd.motorCmd[i].mode = 0X0A; // alwasy set it to 0X0A
        _lowCmd.motorCmd[i].q = cmd->motorCmd[i].q;
        _lowCmd.motorCmd[i].dq = cmd->motorCmd[i].dq;
        _lowCmd.motorCmd[i].tau = cmd->motorCmd[i].tau;
        _lowCmd.motorCmd[i].Kd = cmd->motorCmd[i].Kd;
        _lowCmd.motorCmd[i].Kp = cmd->motorCmd[i].Kp;
    }
    for(int m = 0; m < 10; m++){
        _servo_pub[m].publish(_lowCmd.motorCmd[m]);
    }

}

void CheatIO::recvState(LowlevelState *state)
{
    for(int i = 0; i < 10; i++)
    {
        state->motorState[i].q = _highState.motorState[i].q;
        state->motorState[i].dq = _highState.motorState[i].dq;
        state->motorState[i].tauEst = _highState.motorState[i].tauEst;
    }
    for(int i = 0; i < 3; i++){
        state->imu.quaternion[i] = _highState.imu.quaternion[i];
        state->imu.gyroscope[i] = _highState.imu.gyroscope[i];
        state->position[i] = _highState.position[i];
        state->vWorld[i] = _highState.velocity[i];
    }
    state->imu.quaternion[3] = _highState.imu.quaternion[3];
}

void CheatIO::initSend(){
    _servo_pub[0] = _nm.advertise<unitree_legged_msgs::MotorCmd>( "L_hip_controller/command", 1);
    _servo_pub[1] = _nm.advertise<unitree_legged_msgs::MotorCmd>( "L_hip2_controller/command", 1);
    _servo_pub[2] = _nm.advertise<unitree_legged_msgs::MotorCmd>( "L_thigh_controller/command", 1);
    _servo_pub[3] = _nm.advertise<unitree_legged_msgs::MotorCmd>( "L_calf_controller/command", 1);
    _servo_pub[4] = _nm.advertise<unitree_legged_msgs::MotorCmd>( "L_toe_controller/command", 1);
    _servo_pub[5] = _nm.advertise<unitree_legged_msgs::MotorCmd>( "R_hip_controller/command", 1);
    _servo_pub[6] = _nm.advertise<unitree_legged_msgs::MotorCmd>( "R_hip2_controller/command", 1);
    _servo_pub[7] = _nm.advertise<unitree_legged_msgs::MotorCmd>( "R_thigh_controller/command", 1);
    _servo_pub[8] = _nm.advertise<unitree_legged_msgs::MotorCmd>( "R_calf_controller/command", 1);
    _servo_pub[9] = _nm.advertise<unitree_legged_msgs::MotorCmd>( "R_toe_controller/command", 1);
}

void CheatIO::initRecv(){
    _receive_data_sub = _nm.subscribe("/serial_receive_data", 1, &CheatIO::ReceiveCallback, this);
}

void CheatIO::ReceiveCallback(const msg_process::receive_data &msg)
{

    _highState.position[0] = msg.position_x;
    _highState.position[1] = msg.position_y;
    _highState.position[2] = msg.position_z;
    // 线速度
    _highState.velocity[0] = msg.gyroscope_x;
    _highState.velocity[1] = msg.gyroscope_y;
    _highState.velocity[2] = msg.gyroscope_z;

    _highState.imu.quaternion[0] = msg.quaternion_w;
    _highState.imu.quaternion[1] = msg.quaternion_x;
    _highState.imu.quaternion[2] = msg.quaternion_y;
    _highState.imu.quaternion[3] = msg.quaternion_z;
    // 角速度
    _highState.imu.gyroscope[0] = msg.accelerometer_x;
    _highState.imu.gyroscope[1] = msg.accelerometer_y;
    _highState.imu.gyroscope[2] = msg.accelerometer_z;
    // Lhip
    _highState.motorState[0].mode = msg.mode_L_hip;
    _highState.motorState[0].q = msg.q_L_hip;
    _highState.motorState[0].dq = msg.dq_L_hip;
    _highState.motorState[0].tauEst = msg.tauEst_L_hip;
    // Lhip2
    _highState.motorState[1].mode = msg.mode_L_hip2;
    _highState.motorState[1].q = msg.q_L_hip2;
    _highState.motorState[1].dq = msg.dq_L_hip2;
    _highState.motorState[1].tauEst = msg.tauEst_L_hip2;

    // Lthigh
    _highState.motorState[2].mode = msg.mode_L_thign;
    _highState.motorState[2].q = msg.q_L_thign;
    _highState.motorState[2].dq = msg.dq_L_thign;
    _highState.motorState[2].tauEst = msg.tauEst_L_thign;

    // Lcalf
    _highState.motorState[3].mode = msg.mode_L_calf;
    _highState.motorState[3].q = msg.q_L_calf;
    _highState.motorState[3].dq = msg.dq_L_calf;
    _highState.motorState[3].tauEst = msg.tauEst_L_calf;

    // Ltoe
    _highState.motorState[4].mode = msg.mode_L_toe;
    _highState.motorState[4].q = msg.q_L_toe;
    _highState.motorState[4].dq = msg.dq_L_toe;
    _highState.motorState[4].tauEst = msg.tauEst_L_toe;

    // Rhip
    _highState.motorState[5].mode = msg.mode_R_hip;
    _highState.motorState[5].q = msg.q_R_hip;
    _highState.motorState[5].dq = msg.dq_R_hip;
    _highState.motorState[5].tauEst = msg.tauEst_R_hip;
    // Rhip2
    _highState.motorState[6].mode = msg.mode_R_hip2;
    _highState.motorState[6].q = msg.q_R_hip2;
    _highState.motorState[6].dq = msg.dq_R_hip2;
    _highState.motorState[6].tauEst = msg.tauEst_R_hip2;

    // Rthigh
    _highState.motorState[7].mode = msg.mode_R_thign;
    _highState.motorState[7].q = msg.q_R_thign;
    _highState.motorState[7].dq = msg.dq_R_thign;
    _highState.motorState[7].tauEst = msg.tauEst_R_thign;

    // Rcalf
    _highState.motorState[8].mode = msg.mode_R_calf;
    _highState.motorState[8].q = msg.q_R_calf;
    _highState.motorState[8].dq = msg.dq_R_calf;
    _highState.motorState[8].tauEst = msg.tauEst_R_calf;

    // Rtoe
    _highState.motorState[9].mode = msg.mode_R_toe;
    _highState.motorState[9].q = msg.q_R_toe;
    _highState.motorState[9].dq = msg.dq_R_toe;
    _highState.motorState[9].tauEst = msg.tauEst_R_toe;
}
